#pragma config(Sensor, in1,    leftLine,       sensorLineFollower)
#pragma config(Sensor, in2,    middleLine,     sensorLineFollower)
#pragma config(Sensor, in3,    rightLine,      sensorLineFollower)
#pragma config(Sensor, dgtl1,  rightEncoder,   sensorQuadEncoder)
#pragma config(Sensor, dgtl3,  leftEncoder,    sensorQuadEncoder)
#pragma config(Motor,  port1,           leftMotor,     tmotorVex393, openLoop, reversed)
#pragma config(Motor,  port2,           turntable,     tmotorVex393, openLoop)
#pragma config(Motor,  port10,          rightMotor,    tmotorVex393, openLoop)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

void goRoom4();

task main()
{

	while(true){

		if(vexRT[Btn8U]){
			goRoom4();
		}

	}

}

void goRoom4(){

	SensorValue[rightEncoder] = 0;
	SensorValue[leftEncoder] = 0;

	while(SensorValue[middleLine] < 2500 || SensorValue[rightLine] < 2500)
	{
		if(SensorValue[middleLine] > 2500)
		{
			motor[leftMotor] = 80;
			motor[rightMotor] = 80;
			//Move forward
		}

		else if(SensorValue[leftLine] > 2500)
		{
			motor[leftMotor] = 50;
			motor[rightMotor] = 80;
			//Turn slightly right
		}

		else if (SensorValue[rightLine] > 2500)
		{
			motor[leftMotor] = 80;
			motor[rightMotor] = 50;
			//Turn slightly left
		}
	}

	motor[leftMotor] = 0; //set motors to 0
	motor[rightMotor] = 0;
	wait1Msec(500);
	SensorValue[leftEncoder] = 0; //set encoders to 0
	SensorValue[rightEncoder] = 0;

	while(SensorValue[middleLine] < 2500 || SensorValue[rightLine] < 2500)
	{
		if(SensorValue[middleLine] > 2500)
		{
			motor[leftMotor] = 80;
			motor[rightMotor] = 80;
			//Move forward
		}

		else if(SensorValue[leftLine] > 2500)
		{
			motor[leftMotor] = 50;
			motor[rightMotor] = 80;
			//Turn slightly right
		}

		else if (SensorValue[rightLine] > 2500)
		{
			motor[leftMotor] = 80;
			motor[rightMotor] = 50;
			//Turn slightly left
		}
	}
	motor[leftMotor] = 0; //set motors to 0
	motor[rightMotor] = 0;
	wait1Msec(500);
	SensorValue[leftEncoder] = 0; //set encoders to 0
	SensorValue[rightEncoder] = 0;

	while(SensorValue[middleLine] < 2500 || SensorValue[rightLine] < 2500)
	{
		if(SensorValue[middleLine] > 2500)
		{
			motor[leftMotor] = 80;
			motor[rightMotor] = 80;
			//Move forward
		}

		else if(SensorValue[leftLine] > 2500)
		{
			motor[leftMotor] = 50;
			motor[rightMotor] = 80;
			//Turn slightly right
		}

		else if (SensorValue[rightLine] > 2500)
		{
			motor[leftMotor] = 80;
			motor[rightMotor] = 50;
			//Turn slightly left
		}
	}

	motor[leftMotor] = 0; //set motors to 0
	motor[rightMotor] = 0;
	wait1Msec(500);
	SensorValue[leftEncoder] = 0; //set encoders to 0
	SensorValue[rightEncoder] = 0;

	while(SensorValue[middleLine] < 2500 || SensorValue[rightLine] < 2500)
	{
		if(SensorValue[middleLine] > 2500)
		{
			motor[leftMotor] = 80;
			motor[rightMotor] = 80;
			//Move forward
		}

		else if(SensorValue[leftLine] > 2500)
		{
			motor[leftMotor] = 50;
			motor[rightMotor] = 80;
			//Turn slightly right
		}

		else if (SensorValue[rightLine] > 2500)
		{
			motor[leftMotor] = 80;
			motor[rightMotor] = 50;
			//Turn slightly left
		}
	}
	wait1Msec(215);

	motor[leftMotor] = 0; //set motors to 0
	motor[rightMotor] = 0;
	wait1Msec(500);
	SensorValue[leftEncoder] = 0; //set encoders to 0
	SensorValue[rightEncoder] = 0;

	//FINISH FIRST LENGTH
	//START TURN
	while(SensorValue[rightEncoder]> -283 || SensorValue[leftEncoder]< 283)
	{
		motor[leftMotor] = 80;
		motor[rightMotor] = -80;

		if(SensorValue[rightEncoder]< -283)
		{
			motor[rightMotor] = 0;
		}
		if(SensorValue[leftEncoder]> 283)
		{
			motor[leftMotor] = 0;
		}

	}

	motor[leftMotor] = 0;
	motor[rightMotor] = 0;
	wait1Msec(1000);
	SensorValue[leftEncoder] = 0;
	SensorValue[rightEncoder] = 0;


	//START SECOND LENGTH
	while(SensorValue[middleLine] < 2500 || SensorValue[rightLine] < 2500)
	{
		if(SensorValue[middleLine] > 2500)
		{
			motor[leftMotor] = 80;
			motor[rightMotor] = 80;
			//Move forward
		}

		else if(SensorValue[leftLine] > 2500)
		{
			motor[leftMotor] = 50;
			motor[rightMotor] = 80;
			//Turn slightly right
		}

		else if (SensorValue[rightLine] > 2500)
		{
			motor[leftMotor] = 80;
			motor[rightMotor] = 50;
			//Turn slightly left
		}
	}
	wait1Msec(215);

	motor[leftMotor] = 0; //set motors to 0
	motor[rightMotor] = 0;
	wait1Msec(500);
	SensorValue[leftEncoder] = 0; //set encoders to 0
	SensorValue[rightEncoder] = 0;

	//RETURNING
	//START TURN
	while(SensorValue[rightEncoder]> -283 || SensorValue[leftEncoder]< 283)
	{
		motor[leftMotor] = 80;
		motor[rightMotor] = -80;

		if(SensorValue[rightEncoder]< -283)
		{
			motor[rightMotor] = 0;
		}
		if(SensorValue[leftEncoder]> 283)
		{
			motor[leftMotor] = 0;
		}

	}
	motor[leftMotor] = 0; //set motors to 0
	motor[rightMotor] = 0;
	wait1Msec(500);
	SensorValue[leftEncoder] = 0; //set encoders to 0
	SensorValue[rightEncoder] = 0;

	//START THIRD LENGTH
	while(SensorValue[middleLine] < 2500 || SensorValue[rightLine] < 2500)
	{
		if(SensorValue[middleLine] > 2500)
		{
			motor[leftMotor] = 80;
			motor[rightMotor] = 80;
			//Move forward
		}

		else if(SensorValue[leftLine] > 2500)
		{
			motor[leftMotor] = 50;
			motor[rightMotor] = 80;
			//Turn slightly right
		}

		else if (SensorValue[rightLine] > 2500)
		{
			motor[leftMotor] = 80;
			motor[rightMotor] = 50;
			//Turn slightly left
		}
	}
	wait1Msec(215);

	motor[leftMotor] = 0; //set motors to 0
	motor[rightMotor] = 0;
	wait1Msec(500);
	SensorValue[leftEncoder] = 0; //set encoders to 0
	SensorValue[rightEncoder] = 0;

	//START TURN
	while(SensorValue[rightEncoder]> -283 || SensorValue[leftEncoder]< 283)
	{
		motor[leftMotor] = 80;
		motor[rightMotor] = -80;

		if(SensorValue[rightEncoder]< -283)
		{
			motor[rightMotor] = 0;
		}
		if(SensorValue[leftEncoder]> 283)
		{
			motor[leftMotor] = 0;
		}

	}
	motor[leftMotor] = 0; //set motors to 0
	motor[rightMotor] = 0;
	wait1Msec(500);
	SensorValue[leftEncoder] = 0; //set encoders to 0
	SensorValue[rightEncoder] = 0;

	//START FOURTH LENGTH
	while(SensorValue[middleLine] < 2500 || SensorValue[rightLine] < 2500)
	{
		if(SensorValue[middleLine] > 2500)
		{
			motor[leftMotor] = 80;
			motor[rightMotor] = 80;
			//Move forward
		}

		else if(SensorValue[leftLine] > 2500)
		{
			motor[leftMotor] = 50;
			motor[rightMotor] = 80;
			//Turn slightly right
		}

		else if (SensorValue[rightLine] > 2500)
		{
			motor[leftMotor] = 80;
			motor[rightMotor] = 50;
			//Turn slightly left
		}
	}
	wait1Msec(215);

	motor[leftMotor] = 0; //set motors to 0
	motor[rightMotor] = 0;
	wait1Msec(500);
	SensorValue[leftEncoder] = 0; //set encoders to 0
	SensorValue[rightEncoder] = 0;

	//START TURN
	while(SensorValue[leftEncoder]> -287 || SensorValue[rightEncoder]< 287)
	{
		motor[rightMotor] = 80;
		motor[leftMotor] = -80;

		if(SensorValue[leftEncoder]< -287)
		{
			motor[leftMotor] = 0;
		}
		if(SensorValue[rightEncoder]> 287)
		{
			motor[rightMotor] = 0;
		}

	}
	motor[leftMotor] = 0;
	motor[rightMotor] = 0;
	wait1Msec(1000);
	SensorValue[leftEncoder] = 0;
	SensorValue[rightEncoder] = 0;

	//START FIFTH LENGTH

	while(SensorValue[rightEncoder]< 1232 || SensorValue[leftEncoder]< 1232 )		// Creates an infinite loop, since "true" always evaluates to true
	{
		if(SensorValue[rightEncoder] == SensorValue[leftEncoder]) // If rightEncoder has counted the same amount as leftEncoder:
		{
			// Move Forward
			motor[rightMotor] = 80;
			motor[leftMotor]  = 80;
		}
		else if(SensorValue[rightEncoder] > SensorValue[leftEncoder])	// If rightEncoder has counted more encoder counts
		{
			// Turn slightly right
			motor[rightMotor] = 50;
			motor[leftMotor]  = 80;
		}
		else	// Only runs if leftEncoder has counted more encoder counts
		{
			// Turn slightly left
			motor[rightMotor] = 80;
			motor[leftMotor]  = 50;
		}
	}
	motor[leftMotor] = 0;
	motor[rightMotor] = 0;
	wait1Msec(1000);
}
//FINISH TASK
